/**
 * @file sdp_test.cpp
 * @brief Point Cloud CLIPPER SDR tests
 * @author Parker Lusk <plusk@mit.edu>
 * @date 12 June 2023
 */

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <clipper/clipper.h>
#include <clipper/utils.h>

TEST(CLIPPERSDR, FindGlobalMax) {

  Eigen::MatrixXd M(20, 20);
  M << 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.2964, 0.    ,
       0.    , 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0138, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0016, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0747, 0.    ,
       0.    , 0.    , 0.    , 1.    , 0.    , 0.0555, 0.2547, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0102, 0.    , 0.7715, 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 1.    , 0.0063, 0.    , 0.3846, 0.    , 0.0003, 0.0014, 0.    , 0.    , 0.    , 0.    , 0.0063, 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.0555, 0.0063, 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.9927, 0.    , 0.    , 0.9722, 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.2547, 0.    , 0.    , 1.    , 0.    , 0.0023, 0.    , 0.    , 0.8775, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.3846, 0.    , 0.    , 1.    , 0.0001, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0023, 0.0001, 1.    , 0.7914, 0.    , 0.    , 0.    , 0.0617, 0.    , 0.    , 0.9938, 0.    , 0.    , 0.0007,
       0.    , 0.    , 0.    , 0.    , 0.0003, 0.    , 0.    , 0.    , 0.7914, 1.    , 0.    , 0.    , 0.0001, 0.0091, 0.    , 0.2503, 0.0222, 0.0549, 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.0014, 0.    , 0.    , 0.    , 0.    , 0.    , 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0008,
       0.    , 0.    , 0.0016, 0.    , 0.    , 0.    , 0.8775, 0.    , 0.    , 0.    , 0.    , 1.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.7007, 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.9927, 0.    , 0.    , 0.    , 0.0001, 0.    , 0.    , 1.    , 0.    , 0.9978, 0.    , 0.    , 0.    , 0.    , 0.    ,
       0.    , 0.0138, 0.    , 0.0102, 0.    , 0.    , 0.    , 0.    , 0.0617, 0.0091, 0.    , 0.    , 0.    , 1.    , 0.    , 0.    , 0.    , 0.0003, 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.9978, 0.    , 1.    , 0.0012, 0.    , 0.    , 0.    , 0.0074,
       0.    , 0.    , 0.    , 0.7715, 0.0063, 0.9722, 0.    , 0.    , 0.    , 0.2503, 0.    , 0.    , 0.    , 0.    , 0.0012, 1.    , 0.0026, 0.0217, 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.9938, 0.0222, 0.    , 0.    , 0.    , 0.    , 0.    , 0.0026, 1.    , 0.    , 0.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0549, 0.    , 0.    , 0.    , 0.0003, 0.    , 0.0217, 0.    , 1.    , 0.0007, 0.    ,
       0.2964, 0.    , 0.0747, 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.7007, 0.    , 0.    , 0.    , 0.    , 0.    , 0.0007, 1.    , 0.    ,
       0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.    , 0.0007, 0.    , 0.0008, 0.    , 0.    , 0.    , 0.0074, 0.    , 0.    , 0.    , 0.    , 1.    ;

  Eigen::MatrixXd C = Eigen::MatrixXd::Ones(M.rows(), M.cols());
  C = (M.array() > 0).select(C, M);

  //
  // Algorithm setup
  //

  // instantiate the invariant function that will be used to score associations
  clipper::invariants::EuclideanDistance::Params iparams;
  clipper::invariants::EuclideanDistancePtr invariant =
            std::make_shared<clipper::invariants::EuclideanDistance>(iparams);

  clipper::Params params;
  clipper::CLIPPER clipper(invariant, params);

  clipper.setMatrixData(M, C);

  clipper.solve();
  clipper.solveAsMSRCSDR();

  // clipper::Solution soln = clipper.getSolution();
  // for (const auto& n : soln.nodes) {
  //   std::cout << n << std::endl;
  // }

}


